from collections import defaultdict
from ..externals import six
from ..utils.validation import check_is_fitted
from ..utils import check_random_state, gen_batches, check_array
from ..base import BaseEstimator, ClusterMixin
from ..neighbors import NearestNeighbors
from ..metrics.pairwise import pairwise_distances_argmin
from ..utils._joblib import Parallel
from ..utils._joblib import delayed

class KMedoids:
    def __init__(self, n_clusters=3, max_iter=100, random_state=None):
        self.n_clusters = n_clusters
        self.max_iter = max_iter
        self.random_state = random_state

    def fit(self, X):
        np.random.seed(self.random_state)
        n_samples, n_features = X.shape

        # 随机初始化medoids
        medoid_indices = np.random.choice(n_samples, self.n_clusters, replace=False)
        medoids = X[medoid_indices]

        for _ in range(self.max_iter):
            # 分配每个点到最近的medoid
            clusters = [[] for _ in range(self.n_clusters)]
            for i in range(n_samples):
                distances = np.sqrt(((X[i] - medoids) ** 2).sum(axis=1))
                closest_medoid_idx = np.argmin(distances)
                clusters[closest_medoid_idx].append(i)

            # 更新medoids
            new_medoids = np.copy(medoids)
            for j in range(self.n_clusters):
                if len(clusters[j]) > 0:
                    cluster_points = X[clusters[j]]
                    # 寻找使总距离最小的点作为新的medoid
                    min_total_distance = np.inf
                    for point_idx in clusters[j]:
                        current_point = X[point_idx]
                        total_distance = np.sqrt(((cluster_points - current_point) ** 2).sum(axis=1)).sum()
                        if total_distance < min_total_distance:
                            min_total_distance = total_distance
                            new_medoids[j] = current_point

            # 检查是否收敛
            if np.array_equal(new_medoids, medoids):
                break
            medoids = new_medoids

        # 为每个点分配簇标签
        labels = np.zeros(n_samples, dtype=int)
        for j in range(self.n_clusters):
            for point_idx in clusters[j]:
                labels[point_idx] = j

        self.medoids_ = medoids
        self.labels_ = labels
        return self

    def predict(self, X):
        n_samples = X.shape[0]
        labels = np.zeros(n_samples, dtype=int)
        for i in range(n_samples):
            distances = np.sqrt(((X[i] - self.medoids_) ** 2).sum(axis=1))
            closest_medoid_idx = np.argmin(distances)
            labels[i] = closest_medoid_idx
        return labels

def node():
    # Your existing code here
    while not rospy.is_shutdown():
        # -------------------------------------------------------------------------    
        # Clustering frontier points
        centroids = []
        quantityfrontiers_recrod = []
        quantity_recrod = []
        front = copy(frontiers)

        quantityfrontiers_recrod.append(len(frontiers))
        rospy.loginfo("quantityfrontiers_recrod:" + str(quantityfrontiers_recrod))             
        quantity_recrod.append(len(front))
        rospy.loginfo("quantity_recrod:" + str(quantity_recrod))

        if len(front) > 1:
            # 替换为 KMedoids 聚类
            kmedoids = KMedoids(n_clusters=3, max_iter=100, random_state=0)  # 根据需要调整 n_clusters
            kmedoids.fit(front)
            centroids = kmedoids.medoids_  # 使用 KMedoids 的 medoids 作为聚类中心

        # 如果只有一个前沿点，无需聚类，即 centroids = frontiers
        if len(front) == 1:
            centroids = front
        frontiers = copy(centroids)

if __name__ == "__main__":
    node()